# !/usr/bin/env python
# -*- coding: utf-8 -*-
# @Time    : 2024/1/30 9:13
# @Author  : xinyingjie
# @File    : main-dev.py
# @Description :  隧道多断面仿真拼接


from __future__ import absolute_import
from __future__ import print_function

import json
import os
import threading
import uuid
import random
import time
import asyncio
import traci.constants as tc
import sumolib
import traci
from util.websocketserver1 import WebSocketServer
import kafka_util
from util import sumo_utils
from kafka_util import pre_data_queue, all_data_queue
import logging
from util.tools import calculate_distance

# 全局中心点
GLOBAL_POLYGON_CENTER = [(1612, 866), (1545, 657),
                         (1977, 865), (1994, 1006)]

MAX_SPEED = int(100 / 3.6)
MIN_SPEED = int(60 / 3.6)
CAR_DEFAULT_SPEED = 19
TRUCK_DEFAULT_SPEED = 20
SPECIAL_DEFAULT_SPEED = 18
# 经度逐渐减少
# erId_dict = {
#              "G152300000000000002": {"lon": 121.36065692182399, "lat": 28.213511497617233},
#              "G152300000000000005": {"lon": 121.35657104924599, "lat": 28.21199712635385},
#              "G152300000000000008": {"lon": 121.35252104457169, "lat": 28.210520318595215},
#              "G152300000000000011": {"lon": 121.34842130918594, "lat": 28.209029146437373},
#              "G152300000000000014": {"lon": 121.34409060647153, "lat": 28.207446251920615}}


erId_dict = {
    "S001933030055210010": {"lon": 121.36065692182399, "lat": 28.213511497617233},
    "S001933030055210011": {"lon": 121.35657104924599, "lat": 28.21199712635385},
    "S001933030055210012": {"lon": 121.35252104457169, "lat": 28.210520318595215},
    "S001933030055210013": {"lon": 121.34842130918594, "lat": 28.209029146437373},
    "S001933030055210014": {"lon": 121.34409060647153, "lat": 28.207446251920615}}
route_list = ['1', '36', '46', '44', '-13', '-24', '-25', '-37#0', '-37#1', '-37#2', '-16#0', '-16#1', '-16#2', '-39',
              '-41#0', '-41#1', '-41#2', '-34#0', '-34#1', '-34#2', '-54', '-55', '-28', '-47', '-9', '-26', '-43',
              '-15']
car_cache_info = {}
moveTo_info = {}
routeIdList = []

# "G152300000000000001": {"lon": 121.363835720355738, "lat": 28.2150032304759},
def get_logger(log_type):
    # 第一步，创建一个logger
    logger = logging.getLogger()
    logger.setLevel(logging.INFO)  # Log等级总开关
    # 第二步，创建一个handler，用于写入日志文件
    rq = str(time.strftime('%Y%m%d', time.localtime(time.time()))) + '_' + str(log_type)
    log_path = 'logs/'
    # log_path = '/opt/python' + '/logs/'
    # print('log_path', log_path)
    if not os.path.exists(log_path):
        os.mkdir(log_path)

    log_name = log_path + rq + '.log'
    logfile = log_name
    fh = logging.FileHandler(logfile)
    fh.setLevel(logging.DEBUG)  # 输出到file的log等级的开关
    # 第三步，定义handler的输出格式
    formatter = logging.Formatter("%(asctime)s - %(filename)s[line:%(lineno)d] - %(levelname)s: %(message)s")
    fh.setFormatter(formatter)
    # 第四步，将logger添加到handler里面
    logger.addHandler(fh)
    return logger


def main(sumo_cfg_file=None, sumo_net_file=None, gui=True):
    sumo_net_instance = sumolib.net.readNet(sumo_net_file, withInternal=True, withLatestPrograms=True)
    sumo_utils.start_sumo(sumo_cfg_file, False, gui=True)
    # 设置所有车道的最大通行速度
    for id in traci.lane.getIDList():
        traci.lane.setMaxSpeed(id, MAX_SPEED)
    traci.polygon.add('GLOBAL_POLYGON_CENTER', GLOBAL_POLYGON_CENTER, (1, 0, 1))
    traci.polygon.subscribeContext('GLOBAL_POLYGON_CENTER',
                                   tc.CMD_GET_VEHICLE_VARIABLE, 10000,
                                   [tc.VAR_SPEED, tc.VAR_DECEL, tc.VAR_DISTANCE, tc.VAR_TYPE, tc.VAR_POSITION,
                                    tc.VAR_ROAD_ID, tc.VAR_LANE_INDEX, tc.VAR_LANE_ID, tc.VAR_ACCEL, tc.VAR_ANGLE,
                                    tc.VAR_LANE_ID, tc.VAR_ACCELERATION, tc.VAR_LANE_INDEX,
                                    tc.TRAFFICLIGHT_TYPE_STATIC])
    traci.simulation.subscribe([tc.VAR_ARRIVED_VEHICLES_NUMBER,
                                tc.VAR_ARRIVED_VEHICLES_IDS,
                                tc.VAR_DEPARTED_VEHICLES_NUMBER,
                                tc.VAR_DEPARTED_VEHICLES_IDS,
                                tc.VAR_COLLIDING_VEHICLES_NUMBER,
                                tc.VAR_COLLIDING_VEHICLES_IDS])
    # 获取虚拟设备状态kafka数据
    threading.Thread(target=kafka_util.receive_message_of_pre_data).start()
    threading.Thread(target=kafka_util.receive_message_of_all_data).start()
    ws_server = WebSocketServer()
    ws_server.start()

    EdgeID, Pos, LaneIndex = traci.simulation.convertRoad(121.36441605349023, 28.215228797359245, True)

    for k, v in erId_dict.items():
        x, y = traci.simulation.convertGeo(v["lon"], v["lat"], fromGeo=True)
        EdgeIDk, Posk, LaneIndexk = traci.simulation.convertRoad(v["lon"], v["lat"], True)
        erId_dict[k]["x"] = x
        erId_dict[k]["y"] = y
        erId_dict[k]["edge"] = EdgeIDk
        erId_dict[k]["pos"] = Posk


    # routeID = str(uuid.uuid1())
    # traci.route.add(routeID, [EdgeID])
    # routeID1 = str(uuid.uuid1())
    # traci.route.add(routeID1, [EdgeID1])
    # routeID2 = str(uuid.uuid1())
    # traci.route.add(routeID2, [EdgeID2])

    # print(EdgeID, Pos, LaneIndex)
    # print(EdgeID1, Pos1, LaneIndex1)
    # print(EdgeID2, Pos2, LaneIndex2)
    # start_time = None
    logger = get_logger("simulation")
    startSimuTime = int(time.time() * 1000)
    # try:
    loop = asyncio.get_event_loop()
    while (True):

        # try:
        start_time = time.time()
        while not pre_data_queue.empty():
            vInfo = pre_data_queue.get(block=False)
            # print(vInfo)
            carid = str(vInfo["licenseCode"]) + "_" + str((time.time())) if str(
                vInfo["licenseCode"]) == "无号牌" else str(
                vInfo["licenseCode"])
            carid = str(carid.encode("utf-8"))

            traci.vehicle.add(
                vehID=carid,
                routeID="route_main",
                # typeID=str(vInfo["vehicleType"]), depart='0', departPos=Pos,
                typeID=str(1), depart='0', departPos=Pos,
                departLane=str(random.randint(0, 2)), departSpeed=str(CAR_DEFAULT_SPEED))
            traci.vehicle.setSpeed(carid, CAR_DEFAULT_SPEED)
        global_vehicles = traci.polygon.getContextSubscriptionResults('GLOBAL_POLYGON_CENTER')
        # print(global_vehicles.keys())
        delete_list = []
        for k, v in moveTo_info.items():
            # traci.vehicle.setRoute(k, route_list[route_list.index(v):])
            routeID = ""
            if v["edge"] in routeIdList:
                routeID = v["edge"]
            else:
                traci.route.add(v["edge"],route_list[route_list.index(v["edge"]):])
                routeIdList.append(v["edge"])
                routeID = v["edge"]
            print(routeID,traci.route.getEdges(routeID))
            traci.vehicle.add(
                vehID=k,
                routeID=routeID,
                # typeID=str(vInfo["vehicleType"]), depart='0', departPos=Pos,
                typeID=str(1), depart='0', departPos=v["pos"],
                departLane=str(random.randint(0, 2)), departSpeed=str(CAR_DEFAULT_SPEED))
            traci.vehicle.setSpeed(k, CAR_DEFAULT_SPEED)
            delete_list.append(k)
        for d in delete_list:
            del moveTo_info[d]
        while not all_data_queue.empty():
            all_data_info = all_data_queue.get(block=False)
            etcLicense = ""
            if "etcLicense" in all_data_info.keys():
                etcLicense = all_data_info["etcLicense"]
            else:
                if "licenseCode" in all_data_info.keys():
                    etcLicense = all_data_info["licenseCode"]
            # print("收到all-data数据",all_data_info,etcLicense)
            erId = all_data_info["erId"]
            # print(list(map(lambda key: eval(key).decode('utf-8'), global_vehicles.keys())))
            # print(etcLicense, etcLicense in list(map(lambda key: eval(key).decode('utf-8'), global_vehicles.keys())),
            #       erId in erId_dict.keys())
            # print(list(map(lambda key: eval(key).decode('utf-8'), global_vehicles.keys())), erId)
            # print(all_data_info)
            if etcLicense in list(
                    map(lambda key: eval(key).decode('utf-8'), global_vehicles.keys())) and erId in erId_dict.keys():
                erId_lon, erId_lat = erId_dict[erId]["lon"], erId_dict[erId]["lat"]
                etcLicense = str(etcLicense.encode("utf-8"))
                pos = global_vehicles[etcLicense][tc.VAR_POSITION]
                car_lon, car_lat = sumo_utils.xyConversionlatAndLon(sumo_net_instance, pos)
                dis = calculate_distance(car_lon, car_lat, erId_lon, erId_lat)
                # 若门架上报信息时候，仿真车在门架后面，仿真车需要加速，否则，需要减速
                print("门架匹配成功")
                if dis <= 300:

                    car_cache_info[etcLicense] = {"erId_lon": erId_lon, "erId_lat": erId_lat}
                    if erId_lon < car_lon:
                        print("仿真车在门架后面，仿真车需要加速")
                        traci.vehicle.slowDown(etcLicense, MAX_SPEED, 2.0)
                    else:
                        print("仿真车在门架前面，仿真车需要减速速")
                        traci.vehicle.slowDown(etcLicense, MIN_SPEED, 4.0)
                else:
                    print("moveToXY")
                    traci.vehicle.remove(etcLicense)
                    # traci.vehicle.moveToXY(etcLicense, edgeID="", lane=-1, x=erId_dict[erId]["x"],
                    #                        y=erId_dict[erId]["y"], keepRoute=0)
                    # print([str(erId_dict[erId]["edge"]),"-15"])
                    moveTo_info[etcLicense] = {"edge":str(erId_dict[erId]["edge"]),"pos":str(erId_dict[erId]["edge"])}
                    # traci.vehicle.setRoute(etcLicense,["1","-15"])

        car_list = list()
        for vehicle_id, veh_info in dict(global_vehicles).items():
            # print(traci.vehicle.getRoute(vehicle_id))
            pos = veh_info[tc.VAR_POSITION]
            longitude, latitude = sumo_utils.xyConversionlatAndLon(sumo_net_instance, pos)

            # 当仿真车已经到达门架
            if vehicle_id in car_cache_info.keys():
                # car_cache_info[etcLicense] = {"erId_lon": erId_lon, "erId_lat": erId_lat}
                erId_lon = car_cache_info[vehicle_id]["erId_lon"]
                # erId_lat = car_cache_info[vehicle_id]["erId_lat"]
                # dis = calculate_distance(longitude, latitude, erId_lon, erId_lat)
                if longitude <= erId_lon:
                    print("车辆经过门架，恢复初始速度")
                    traci.vehicle.slowDown(vehicle_id, CAR_DEFAULT_SPEED, 4.0)
                    traci.vehicle.setSpeed(vehicle_id, CAR_DEFAULT_SPEED)
                    del car_cache_info[vehicle_id]

            pos = veh_info[tc.VAR_POSITION]
            # longitude, latitude = sumo_utils.xyConversionlatAndLon(sumo_net_instance, pos)
            speed = veh_info[tc.VAR_SPEED]
            print(speed)
            vehicle_type = veh_info[tc.VAR_TYPE]
            vehicle_id = eval(vehicle_id).decode('utf-8')
            car = {"id": vehicle_id, "typeCode": vehicle_type, "longitude": longitude,
                   "latitude": latitude,
                   "angle": veh_info[tc.VAR_ANGLE],
                   "licenseCode": vehicle_id if "_" not in vehicle_id else "默A0000",
                   "eventType": "", "cl": ""}

            car_list.append(car)
        timestamp = int(time.time() * 1000)
        result = {"objs": car_list, "participantNum": len(car_list),
                  "timestamp": timestamp,
                  "globalTimeStamp": startSimuTime, "simuTime": timestamp - startSimuTime,
                  "count": len(car_list)}
        # print(result)
        loop.run_until_complete(ws_server.broadcast(str(result).replace("'", '"')))
        traci.simulationStep()
        end_time = time.time()

        if start_time and (end_time - start_time) < 0.1:
            # print("耗时",(end_time - start_time))
            time.sleep(0.1 - (end_time - start_time))
        else:
            if start_time:
                print("单帧耗时大于0.1s", end_time - start_time)
    #         except Exception as e:
    #             traci.simulationStep()
    #             time.sleep(0.05)
    #             print(str(e))
    #             continue
    #
    # except Exception as e:
    #     print(str(e))


if __name__ == "__main__":
    sumo_net_file = r"D:\工作\项目\浙江大桥\project\zhejaing_sumo\config\longshan.net.xml"  # 路网文件
    sumo_cfg_file = r"D:\工作\项目\浙江大桥\project\zhejaing_sumo\config\longshan.sumocfg"  # 路网文件
    logger = get_logger("simulation")
    logger.info("test")

    main(sumo_cfg_file=sumo_cfg_file, sumo_net_file=sumo_net_file)
    # sumo_net_file = "../config/0815-v1.net.xml"  # 路网文件
    # sumo_cfg_file = "../config/longshan.sumocfg"  # 路网文件
    # main(sumo_cfg_file=sumo_cfg_file, sumo_net_file=sumo_net_file)
